#include "radius_filter.h"
namespace perception
{
    namespace lidar_preprocess
    {
        template <typename T1, typename T2, typename T3>
        void RadiusFilter<T1, T2, T3>::Process(typename pcl::PointCloud<T1>::Ptr &data_in, typename pcl::PointCloud<T2>::Ptr &data_out, T3 &config)
        {
            if (data_in->points.empty())
            {
                std::cout << "no lidar points input\n";
                return;
            }
            typename pcl::PointCloud<T2>::Ptr cloud(new pcl::PointCloud<T2>);
            pcl::copyPointCloud(*data_in, *cloud);
            pcl::RadiusOutlierRemoval<T2> *outrem = new pcl::RadiusOutlierRemoval<T2>;
            outrem->setInputCloud(cloud);
            outrem->setRadiusSearch(config.radius_filter_config_.radius_search);      // 搜索半径
            outrem->setMinNeighborsInRadius(config.radius_filter_config_.search_num); // 搜索 最少点数
            outrem->filter(*data_out);
            delete outrem;
        }
        INSTANTIATE_PRODUCT(INSTANTIATE_RadiusFilter, LIDAR_PREPROCESS_TYPES)
    }
}